#include "tcptovrep.h"

#define IF_MULTI_ROUND true

TcpToVrep::TcpToVrep(QTcpSocket *parent) :
    QTcpSocket(parent)
{
    remoteIpAddress="192.168.1.255";
}

void TcpToVrep::connectToVrep()
{
    connectToHost( remoteIpAddress, 15000 );
}

void TcpToVrep::move_axies(QVector<float> val)
{

    if(!this->isOpen() || this->state()!=QTcpSocket::ConnectedState )
    {
        return;
    }

    S_CLIENT_DATA dataToSev;
    dataToSev.vecSize=val.size();
    dataToSev.vecData=val;

    QByteArray buf;
    QDataStream ind( &buf, QIODevice::WriteOnly );
    ind.setVersion( QDataStream::Qt_4_6 );

    dataToSev.pkgLen= 0;
    dataToSev.cmdType= 0;

    ind << dataToSev.pkgLen;
    ind << dataToSev.cmdType;
    ind << dataToSev.vecSize;
    ind << dataToSev.vecData;

    ind.device()->seek(0);
    dataToSev.pkgLen = buf.size()-D_COMM_SIZEOF_PKG_LEN ;
    ind<< dataToSev.pkgLen ; //重新修改包大小

#if 0
    bufToServer += buf;

    if( bufToServer.size() < 40 )
        return;

    write( bufToServer );
    bufToServer.clear();
#else
    if( buf.size() < 40 )
        return;
    write( buf );
#endif

}


QVector<float> TcpToVrep::get_axies_data()
{
    QVector<float> data;
    if(!this->isOpen()  || this->state()!=QTcpSocket::ConnectedState )
    {
        return data;
    }

    if( bytesAvailable() < 40)
        return data;

    bufFromServer += readAll();

    S_CLIENT_DATA dataToSev;
    QDataStream ind( &bufFromServer, QIODevice::ReadOnly );
    ind.setVersion( QDataStream::Qt_4_6 );

    dataToSev.pkgLen= 0;
    dataToSev.cmdType= 0;

    ind >> dataToSev.pkgLen;
    ind >> dataToSev.cmdType;
    ind >> dataToSev.vecSize;
    ind >> dataToSev.vecData;
    // ind >> dataToSev.pkgLen ; //重新修改包大小
    //qDebug()<< dataToSev.vecData;
    bufFromServer.clear(); //防止缓存过多

//    if( dataToSev.vecSize==dataToSev.vecData.size() )
    if(IF_MULTI_ROUND)
    {
        static float lastVec;
        static int roundVec;
        static bool isInit =false;
        if(isInit)
        {
            if((lastVec>2)&&(dataToSev.vecData[3]<-2)){
                roundVec++;
            }
            else if((lastVec<-2)&&(dataToSev.vecData[3]>2)){
                roundVec--;
            }
            lastVec = dataToSev.vecData[3];
            dataToSev.vecData[3] += 3.14159265358979323846*2*roundVec;
        }
        else
        {
            isInit = true;
            lastVec = dataToSev.vecData[3];
            roundVec = 0;
        }
    }

    return dataToSev.vecData;
}
